﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;


public partial class CubicInterpolation
{
    public enum Limits
    {
        Nature,//S'' = 0 ,边际点二次导数为0
        NotAKnot,//
        Clamped,
    }
    
    LinearEquationSolving linearEquationSolving;

    Unknown[] A;
    public Unknown[] a => A;
    Unknown[] B;
    public Unknown[] b => B;
    Unknown[] C;
    public Unknown[] c => C;
    Unknown[] D;
    public Unknown[] d => D;

    public CubicInterpolation()
    {

    }

    public CubicInterpolation(FixedVec2[] points)
    {
        linearEquationSolving = CalculateABCDN3(points, out A, out B, out C, out D);
    }
  
    public FuncCurve CubicCurveInterpolation2(FixedVec2[] points)
    {
        if (points == null || points.Length <= 0)
        {
            throw new ArgumentException("点输入不合法！");
        }

        if (points.Length == 1)
            return new FuncCurve((x) => { return points[0].y; });

        Func<FixedFloat, FixedFloat> res = (x) =>
        {
            FixedVec2 beginPoint = points[0];
            if (x == beginPoint.x)
            {
                return beginPoint.y;
            }
            FixedVec2 endPoint = points[points.Length - 1];
            if (x == endPoint.x)
            {
                return endPoint.y;
            }

            FixedVec2 pi = FixedVec2.zero;
            FixedVec2 piAdd1 = FixedVec2.zero;
            FixedVec2 piMinu1 = FixedVec2.zero;

            int index = 0;
            for (int i = 0; i < points.Length; i++)
            {
                if (x < points[i].x)
                {
                    index = i;

                    break;
                }
            }

            if (this.linearEquationSolving != null)
            {

            }
            else
            {
                linearEquationSolving = CalculateABCDN3(points, out A, out B, out C, out D);
            }
            
            if(index >= A.Length)
            {
                GameDebug.LogError($"index >= A.Length  index:{index}   A.Length:{A.Length}");
            }
            if (index >= B.Length)
            {
                GameDebug.LogError($"index >= B.Length  index:{index}   B.Length:{B.Length}");
            }
            if (index >= C.Length)
            {
                GameDebug.LogError($"index >= C.Length  index:{index}   C.Length:{C.Length}");
            }
            if (index >= D.Length)
            {
                GameDebug.LogError($"index >= D.Length  index:{index}   D.Length:{D.Length}");
            }
            FixedFloat a = A[index].value;
            FixedFloat b = B[index].value;
            FixedFloat c = C[index].value;
            FixedFloat d = D[index].value;

            //FixedFloat y = a + b * x + c * x * x + d * x * x * x;
            if(index - 1< 0)
            {
                return beginPoint.y;
            }
            FixedFloat xiMinu1 = points[index - 1].x;
            FixedFloat xDiff = x - xiMinu1;
            FixedFloat y = a + b* xDiff + c * xDiff * xDiff + d * xDiff * xDiff * xDiff;
            return y;
        };

        return new FuncCurve(res);
    }

    /// <summary>
    /// 三点确定一条曲线，（中点处的斜率等于两边的斜率的均值）
    /// </summary>
    /// <param name="points"></param>
    /// <param name="A"></param>
    /// <param name="B"></param>
    /// <param name="C"></param>
    /// <param name="D"></param>
    /// <param name="limits"></param>
    /// <returns></returns>
    static LinearEquationSolving CalculateABCDN3(FixedVec2[] points, out Unknown[] A, out Unknown[] B, out Unknown[] C, out Unknown[] D, Limits limits = Limits.Nature)
    {
        int n = points.Length;
        
        A = new Unknown[n];
        B = new Unknown[n];
        C = new Unknown[n];
        D = new Unknown[n];

        if (n <= 1)
        {
            return null;
        }

        FixedFloat[] X = new FixedFloat[n];
        FixedFloat[] Y = new FixedFloat[n];
        for (int i = 0; i < n; i++)
        {
            A[i] = new Unknown($"A{i}");
            B[i] = new Unknown($"B{i}");
            C[i] = new Unknown($"C{i}");
            D[i] = new Unknown($"D{i}");

            X[i] = points[i].x;
            Y[i] = points[i].y;
        }

        List<LinearEquation> linearEquations = new List<LinearEquation>();
        for (int i = 1; i <= n - 1; i++)
        {
            A[i].knownValue = true;
            A[i].value = Y[i - 1];
        }

        if (limits == Limits.Clamped)
        {
            //两个边际点一次导数为0
            B[1].knownValue = true;
            B[1].value = 0;

            FixedFloat lastDeltaX = X[n - 1] - X[n - 2];
            UnknownExpression l3ue1 = new UnknownExpression(1, B[n - 1]);
            UnknownExpression l3ue2 = new UnknownExpression(2 * lastDeltaX, C[n - 1]);
            UnknownExpression l3ue3 = new UnknownExpression(3 * lastDeltaX * lastDeltaX, D[n - 1]);
            LinearEquation l3 = new LinearEquation(new UnknownExpression[3] { l3ue1, l3ue2, l3ue3 }, 0);
            linearEquations.Add(l3);
        }
        else if (limits == Limits.Nature)
        {
            //两个边际点二次导数为0
            C[1].knownValue = true;
            C[1].value = 0;

            FixedFloat lastDeltaX = X[n - 1] - X[n - 2];
            UnknownExpression l3ue1 = new UnknownExpression(2, C[n - 1]);
            UnknownExpression l3ue2 = new UnknownExpression(6 * lastDeltaX, D[n - 1]);
            LinearEquation l3 = new LinearEquation(new UnknownExpression[2] { l3ue1, l3ue2 }, 0);
            linearEquations.Add(l3);
        }

        ///在点处的斜率等于两侧边的斜率的均值
        for (int i = 2; i <= n - 1; i++)
        {
            FixedFloat k = 0;
            if(X[i - 1] != X[i - 2] && X[i] != X[i - 1])
                k = ((Y[i - 1] - Y[i - 2])/ (X[i - 1] - X[i - 2]) + (Y[i] - Y[i - 1]) / (X[i ] - X[i - 1]))/ 2;

            FixedFloat deltaX = X[i-1] - X[i - 2];
            UnknownExpression l4ue1 = new UnknownExpression(1, B[i-1]);
            UnknownExpression l4ue2 = new UnknownExpression(2 * deltaX, C[i-1]);
            UnknownExpression l4ue3 = new UnknownExpression(3 * deltaX * deltaX, D[i-1]);
            LinearEquation l4 = new LinearEquation(new UnknownExpression[3] { l4ue1, l4ue2, l4ue3 }, -k);
            linearEquations.Add(l4);

            B[i].knownValue = true;
            B[i].value = k;
        }

        for (int i = 1; i <= n - 1; i++)
        {
            FixedFloat deltaX = X[i] - X[i - 1];
            UnknownExpression l5ue1 = new UnknownExpression(1, A[i]);
            UnknownExpression l5ue2 = new UnknownExpression(deltaX, B[i]);
            UnknownExpression l5ue3 = new UnknownExpression(deltaX * deltaX, C[i]);
            UnknownExpression l5ue4 = new UnknownExpression(deltaX * deltaX * deltaX, D[i]);
            LinearEquation l5 = new LinearEquation(new UnknownExpression[4] { l5ue1, l5ue2, l5ue3, l5ue4 }, -Y[i]);
            linearEquations.Add(l5);
        }

        LinearEquationSolving linearEquationSolving = new LinearEquationSolving(linearEquations.ToArray());
        return linearEquationSolving;
    }

    static LinearEquationSolving CalculateABCDN2(FixedVec2[] points, out Unknown[] A, out Unknown[] B, out Unknown[] C, out Unknown[] D, Limits limits = Limits.Nature)
    {
        int n = points.Length;

        A = new Unknown[n];
        B = new Unknown[n];
        C = new Unknown[n];
        D = new Unknown[n];

        FixedFloat[] X = new FixedFloat[n];
        FixedFloat[] Y = new FixedFloat[n];
        for (int i = 0; i < n; i++)
        {
            A[i] = new Unknown($"A{i}");
            B[i] = new Unknown($"B{i}");
            C[i] = new Unknown($"C{i}");
            D[i] = new Unknown($"D{i}");

            X[i] = points[i].x;
            Y[i] = points[i].y;
        }

        List<LinearEquation> linearEquations = new List<LinearEquation>();
        for (int i = 1; i <= n - 1; i++)
        {
            A[i].knownValue = true;
            A[i].value = Y[i - 1];

            //UnknownExpression l0ue1 = new UnknownExpression(1, A[i]);
            //LinearEquation l0 = new LinearEquation(new UnknownExpression[1] { l0ue1 }, -Y[i - 1]);
            //linearEquations.Add(l0);
        }

        if (limits == Limits.Clamped)
        {
            //两个边际点一次倒数为0
            B[1].knownValue = true;
            B[1].value = 0;
            //UnknownExpression l1ue1 = new UnknownExpression(1, B[1]);
            //LinearEquation l1 = new LinearEquation(new UnknownExpression[1] { l1ue1}, 0);
            //linearEquations.Add(l1);

            FixedFloat lastDeltaX = X[n - 1] - X[n - 2];
            UnknownExpression l3ue1 = new UnknownExpression(1, B[n - 1]);
            UnknownExpression l3ue2 = new UnknownExpression(2 * lastDeltaX, C[n - 1]);
            UnknownExpression l3ue3 = new UnknownExpression(3 * lastDeltaX * lastDeltaX, D[n - 1]);
            LinearEquation l3 = new LinearEquation(new UnknownExpression[3] { l3ue1, l3ue2, l3ue3 }, 0);
            linearEquations.Add(l3);
        }
        else if (limits == Limits.Nature)
        {
            //两个边际点二次倒数为0
            C[1].knownValue = true;
            C[1].value = 0;
            //UnknownExpression l1ue1 = new UnknownExpression(1, C[1]);
            //LinearEquation l1 = new LinearEquation(new UnknownExpression[1] { l1ue1 }, 0);
            //linearEquations.Add(l1);

            FixedFloat lastDeltaX = X[n - 1] - X[n - 2];
            UnknownExpression l3ue1 = new UnknownExpression(2, C[n - 1]);
            UnknownExpression l3ue2 = new UnknownExpression(6 * lastDeltaX, D[n - 1]);
            LinearEquation l3 = new LinearEquation(new UnknownExpression[2] { l3ue1, l3ue2 }, 0);
            linearEquations.Add(l3);
        }

        for (int i = 1; i <= n - 2; i++)
        {
            FixedFloat deltaX = X[i] - X[i - 1];
            UnknownExpression l2ue1 = new UnknownExpression(1, C[i]);
            UnknownExpression l2ue2 = new UnknownExpression(3 * deltaX, D[i]);
            UnknownExpression l2ue3 = new UnknownExpression(-1, C[i + 1]);
            LinearEquation l2 = new LinearEquation(new UnknownExpression[3] { l2ue1, l2ue2, l2ue3 }, 0);
            linearEquations.Add(l2);
        }

        for (int i = 1; i <= n - 2; i++)
        {
            FixedFloat deltaX = X[i] - X[i - 1];
            UnknownExpression l4ue1 = new UnknownExpression(1, B[i]);
            UnknownExpression l4ue2 = new UnknownExpression(2 * deltaX, C[i]);
            UnknownExpression l4ue3 = new UnknownExpression(3 * deltaX * deltaX, D[i]);
            UnknownExpression l4ue4 = new UnknownExpression(-1, B[i + 1]);
            LinearEquation l4 = new LinearEquation(new UnknownExpression[4] { l4ue1, l4ue2, l4ue3, l4ue4 }, 0);
            linearEquations.Add(l4);
        }

        for (int i = 1; i <= n - 1; i++)
        {
            FixedFloat deltaX = X[i] - X[i - 1];
            UnknownExpression l5ue1 = new UnknownExpression(1, A[i]);
            UnknownExpression l5ue2 = new UnknownExpression(deltaX, B[i]);
            UnknownExpression l5ue3 = new UnknownExpression(deltaX * deltaX, C[i]);
            UnknownExpression l5ue4 = new UnknownExpression(deltaX * deltaX * deltaX, D[i]);
            LinearEquation l5 = new LinearEquation(new UnknownExpression[4] { l5ue1, l5ue2, l5ue3, l5ue4 }, -Y[i]);
            linearEquations.Add(l5);
        }

        LinearEquationSolving linearEquationSolving = new LinearEquationSolving(linearEquations.ToArray());
        return linearEquationSolving;
    }

}

/// <summary>
/// 定义域内给定x 有唯一点y与之对应
/// </summary>
public class FuncCurve
{
    public FixedFloat m_MinX;
    public FixedFloat minX => m_MinX;
    public FixedFloat m_MaxX;
    public FixedFloat maxX => m_MaxX;

    public Func<FixedFloat, FixedFloat> func;

    public FuncCurve()
    {

    }

    public FuncCurve(Func<FixedFloat, FixedFloat> _func)
    {
        func = _func;
    }
    
    public FixedFloat GetY(FixedFloat x)
    {
        if (func != null)
            return func(x);

        return 0;
    }
}